/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2021-07-15     MBW       the first version
 */
#include "motor.h"
#include "data_define.h"

motorPWM_t motorPWM={0};
/**
  * @brief TIM4 Initialization Function
  * @param None
  * @retval None
  */
void MX_TIM4_Init2(void)
{
    RCC->AHB1ENR |= (1<<1); //打开GPIOB时钟
    GPIO_Set(GPIOB,PIN6|PIN7|PIN8|PIN9,GPIO_MODE_AF,GPIO_OTYPE_PP,GPIO_SPEED_100M,GPIO_PUPD_PD);//复用功能,下拉
    GPIO_AF_Set(GPIOB,6,2); //PB6,AF2
    GPIO_AF_Set(GPIOB,7,2); //PB7,AF2
    GPIO_AF_Set(GPIOB,8,2); //PB8,AF2
    GPIO_AF_Set(GPIOB,9,2); //PB9,AF2
    TIM4_CLOCK_INIT; //打开TIM4时钟
    TIM4->PSC = 83; //对时钟84M进行84分频，使得计数频率为1M
    TIM4->ARR = 4000;  //周期2.5ms
//    TIM4->ARR = 20000;  //周期20ms
   // TIM3->EGR |= 1; //产生一次更新事件

    TIM4->CCMR1 |= 0x6868;//PWM模式1,启动预装载
    TIM4->CCMR2 |= 0x6868;//PWM模式1,启动预装载
    TIM4->CCER |= 0x1111;//使能比较输出

    TIM4->CCR1 = 1000;//设置第一通道PC6
    TIM4->CCR2 = 1000;//设置第二通道PC7
    TIM4->CCR3 = 1000; //设置第三通道PC8
    TIM4->CCR4 = 1000; //设置第四通道PC69

    TIM4->CR1 |= 1; //开始计时

  /* USER CODE END TIM4_Init 2 */
//  HAL_TIM_MspPostInit(&htim4);
  rt_kprintf("TIM4_PWM_init is ok! \n");
}
//INIT_APP_EXPORT(MX_TIM4_Init2);
/* USER CODE END 4 */
void powerControl(motorPWM_t *MotorData,unsigned char state)
{
    if(state)
    {
        motorPWM.m1 = MotorData->thrust +  MotorData->roll + MotorData->pitch - MotorData->yaw + 1000;
        motorPWM.m2 = MotorData->thrust +  MotorData->roll - MotorData->pitch + MotorData->yaw + 1000;
        motorPWM.m3 = MotorData->thrust -  MotorData->roll + MotorData->pitch + MotorData->yaw + 1000;
        motorPWM.m4 = MotorData->thrust -  MotorData->roll - MotorData->pitch - MotorData->yaw + 1000;
        motorPWM.m1 = constrain(motorPWM.m1, min_power, max_power)  ;//减去1000基础值，实际油门应该是0-1000
        motorPWM.m2 = constrain(motorPWM.m2, min_power, max_power)  ;
        motorPWM.m3 = constrain(motorPWM.m3, min_power, max_power)  ;
        motorPWM.m4 = constrain(motorPWM.m4, min_power, max_power)  ;
    }
    else
    {
        motorPWM.m1 = 1000;
        motorPWM.m2 = 1000;
        motorPWM.m3 = 1000;
        motorPWM.m4 = 1000;
    }

        motorsSetRatio(MOTOR_M1, motorPWM.m1);
        motorsSetRatio(MOTOR_M2, motorPWM.m2);
        motorsSetRatio(MOTOR_M3, motorPWM.m3);
        motorsSetRatio(MOTOR_M4, motorPWM.m4);
}
/*设置电机PWM占空比*/
/*ithrust:0-1000*/
void motorsSetRatio(unsigned int id, unsigned int ithrust)
{

        switch(id)
        {
            case 0:     /*MOTOR_M1*/
                TIM4->CCR1 = ithrust;
                break;
            case 1:     /*MOTOR_M2*/
                TIM4->CCR2 = ithrust;
                break;
            case 2:     /*MOTOR_M3*/
                TIM4->CCR3 = ithrust;
                break;
            case 3:     /*MOTOR_M4*/
                TIM4->CCR4 = ithrust;
                break;
            default: break;
        }

}
/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void tim4_pwm_set(int argc, char**argv)
{
    rt_kprintf("**********%s**********\n",argv[1]);
    rt_thread_mdelay(10);
    TIM4->CCR1 = atoi(argv[1]);
    TIM4->CCR2 = atoi(argv[2]);
    TIM4->CCR3 = atoi(argv[3]);
    TIM4->CCR4 = atoi(argv[4]);
    rt_kprintf("CH1 %d   CH2 %d  CH3 %d   CH4 %d  \n",TIM4->CCR1,TIM4->CCR2,TIM4->CCR3,TIM4->CCR4);

}
MSH_CMD_EXPORT(tim4_pwm_set,tim4_pwm_set);
